#include "AS5600.h"


static float angle_data_prev; //�ϴ�λ��
static float full_rotation_offset; //ת������Ȧ��
extern FOC_DEF foc;



unsigned char write_reg(unsigned char reg, unsigned char value)
{
	return HAL_I2C_Mem_Write(&hi2c1, Slave_Addr, reg, I2C_MEMADD_SIZE_8BIT, &value, 1, 50);
}

unsigned char write_regs(unsigned char reg, unsigned char *value, unsigned char len)
{
	return HAL_I2C_Mem_Write(&hi2c1, Slave_Addr, reg, I2C_MEMADD_SIZE_8BIT, value, len, 50);
}

unsigned char read_reg(unsigned char reg, unsigned char* buf, unsigned short len)
{
	return HAL_I2C_Mem_Read(&hi2c1, Slave_Addr, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 50);
}



float i2c_AS5600_get_angle(void)
{   float angle_d;
	int16_t in_angle;
    uint8_t temp[2]={0,0};
	read_reg( Angle_Hight_Register_Addr, temp, 2);
    in_angle = ((int16_t)temp[0] <<8) | (temp[1]);
    angle_d = (float)in_angle * cpr / 4096;
		

		return angle_d;
	//angle_dΪ�����ƣ���Χ��0-6.28	
}
float i2c_AS5600_get_angle_4096(void)
{   
	int16_t in_angle;
    uint8_t temp[2]={0,0};
	read_reg( Angle_Hight_Register_Addr, temp, 2);
    in_angle = ((int16_t)temp[0] <<8) | (temp[1]);
		

	return in_angle;
}
float y_vel_prev;

float LPF_velocity(float x)
{
	float y = 0.9f*y_vel_prev + 0.1f*x;
	
	y_vel_prev=y;
	
	return y;
}
float First_order_Filtering(float input)
{
  float output;
  static float output_last;
  output = 0.9f*output_last + 0.1f*input;
  output_last = output;
  return output;
}
//�Ƕ��ۼƼ���
float val=0.0f,last_val=0.0f,err=0.0f,circle = 0.0f;
float angle_get_all(float angle)
{
	val = angle;
	err = val - last_val;
	if( fabs(err) > (0.8f*6.2831853f) )
		circle += (err > 0) ? -1 : 1;
	last_val = val;
	return (float)circle * 6.28318530718f + last_val;
}


float bsp_as5600GetAngle(void)
{
  float angle_data = i2c_AS5600_get_angle_4096();
  
  float d_angle = angle_data - angle_data_prev;
  if(abs(d_angle) > (0.8 * 4096)) {
    full_rotation_offset += (d_angle > 0 ? -2*PI : 2*PI);
  }
  angle_data_prev = angle_data;
  return (full_rotation_offset + (angle_data / (float)4096)*2*PI);
}
	static float speed_old=0;
	static float err__speed=0;
float get_speed(void)
{
	float angle_b=0;

	err__speed=foc.the-speed_old;
	speed_old=foc.the;
	if(err__speed<-PI)
	{
		err__speed=err__speed+2*PI;
	}
	if(err__speed>PI)
	{
		err__speed=err__speed-2*PI;
	}
	angle_b=err__speed/0.01f;
	return angle_b;
}
	
	
	
	
